#include <iostream>
#include "Color2GrayMapper.h"
//#define VERBOSE

using namespace std;
using namespace cv;

Color2GrayMapper * Color2GrayMapper::me = NULL;

Color2GrayMapper * Color2GrayMapper::GetInstance( void ) {
	if (me == NULL) {
		me = new Color2GrayMapper;
		me->init();
	}
	return me;
}

void Color2GrayMapper::load( void )
{
	FileStorage fs;
	FileNode vecs;

	fs.open("calibrate_color_result.yml", FileStorage::READ);
	fs["intriMat"] >> intriMatC;
	fs["distortMat"] >> distortMatC;
	vecs = fs["size"] ;
	(*vecs.begin()) >> height;
	(*(++vecs.begin())) >> width;

	vecs = fs["rvecs"];
	for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
		Mat tmp;
		(*it) >> tmp;
		rvecsC.push_back(tmp);
	}
	vecs = fs["tvecs"];
	for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
		Mat tmp;
		(*it) >> tmp;
		tvecsC.push_back(tmp);
	}
	fs.release();


	fs.open("calibrate_gray_result.yml", FileStorage::READ);
	fs["intriMat"] >> intriMatG;
	fs["distortMat"] >> distortMatG;
	vecs = fs["rvecs"];
	for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
		Mat tmp;
		(*it) >> tmp;
		rvecsG.push_back(tmp);
	}
	vecs = fs["tvecs"];
	for (FileNodeIterator it = vecs.begin(); it != vecs.end(); ++it) {
		Mat tmp;
		(*it) >> tmp;
		tvecsG.push_back(tmp);
	}
	fs.release();

	fs.open("stereo_result.yml", FileStorage::READ);
	fs["R"] >> R;
	fs["T"] >> T;
	fs["edgeLen"] >> edgeLen;
	fs.release();

}

void Color2GrayMapper::init( void )
{
	load();
	cout << "width: " << width << " height: " << height << endl;

	for (int i=7; i<8; ++i) {
		Rodrigues(rvecsC[i], R1);
		T1 = tvecsC[i];
		// 			ezPrintMat(R1);
		// 			ezPrintMat(T1);

		Rodrigues(rvecsG[i], R2);
		T2 = tvecsG[i];
		// 			ezPrintMat(R2);
		// 			ezPrintMat(T2);

		// 			Mat R = R1.inv();
		// 			Mat T = -R2*R*T1 + T2;
		// 			R = R2*R;
		// 
		// 			ezPrintMat(R);
		// 			ezPrintMat(T);
	}


	k1 = distortMatG.at<double>(0);
	k2 = distortMatG.at<double>(1);
	k3 = distortMatG.at<double>(4);
	p1 = distortMatG.at<double>(2);
	p2 = distortMatG.at<double>(3);
}

Mat Color2GrayMapper::mapping( const Mat& color, const Mat& dcMapped, const Rect& inRect,
	double edgeInMM /*= -1*/ )
{
	Rect tmp;
	return mapping(color, dcMapped, inRect, tmp, edgeInMM);
}

Mat Color2GrayMapper::mapping( const Mat& color, const Mat& dcMapped, const Rect& inRect,
	Rect& outRect, double edgeInMM /*= -1*/ )
{
	double scale = 263 * edgeLen/34;
	Mat mapped = Mat::zeros(height, width, CV_8UC3);

	int top = height, bottom = 0, left = width, right = 0;

	for (int i=inRect.y; i<inRect.y+inRect.height; ++i) {
		for (int j=inRect.x; j<inRect.x+inRect.width; ++j) {
			short dval = dcMapped.at<short>(i,j);
			if (dval <= 100 || dval >= 10000) continue;

			Mat P = (Mat_<double>(3,1) << j,i,1);

			P = mapping(P, dval, scale);

			int px, py;
			px = cvRound(P.at<double>(0));
			py = cvRound(P.at<double>(1));

			if (0<=px && px<width && 0<=py && py<height) {
				mapped.at<Vec3b>(py,px) = color.at<Vec3b>(i,j);
				bool update = false;
				for (int k=0; k<3; ++k)
					if (color.at<Vec3b>(i,j)[k] > 10)
						update = true;
				if (update) {
					if (px < left) left = px;
					if (px > right) right = px;
					if (py < top) top = py;
					if (py > bottom) bottom = py;
				}
			}
		}
	}
	outRect.x = left;
	outRect.y = top;
	outRect.width = right - left;
	outRect.height = bottom - top;
	return mapped;
}

cv::Mat Color2GrayMapper::mapping( const Mat& color, const Mat& dcMapped, double scale /*= 263*/ )
{
	Mat mapped = Mat::zeros(height, width, CV_8UC3);
	for (int i=0; i<height; ++i) {
		for (int j=0; j<width; ++j) {
			short dval = dcMapped.at<short>(i,j);
			if (dval <= 100 || dval >= 10000) continue;

			Mat P = (Mat_<double>(3,1) << j,i,1);

			P = mapping(P, dval, scale);

			int px, py;
			px = cvRound(P.at<double>(0));
			py = cvRound(P.at<double>(1));

			if (0<=px && px<width && 0<=py && py<height) {
				mapped.at<Vec3b>(py,px) = color.at<Vec3b>(i,j);
			}
		}
	}
	return mapped;
}

cv::Mat Color2GrayMapper::mapping( Mat P, short dval, double scale /*= 263*/ )
{
#ifdef VERBOSE
	ezPrintMat(P);
	ezPrintMat(intriMatC.inv()*P);
#endif
	P = intriMatC.inv()*P;
#ifdef VERBOSE
	ezPrintMat(dval/26.1 * P);
#endif
	P = dval/scale * P;

#ifdef VERBOSE
	ezPrintMat(R*P+T);
#endif
	P = R*P+T;

#ifdef VERBOSE
	ezPrintMat(P/P.at<double>(2));
#endif
	P = P/P.at<double>(2);

	double x,y,r;
	x = P.at<double>(0);
	y = P.at<double>(1);
	r = x*x+y*y;
	P.at<double>(0) = x*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p1*x*y + p2*(r+2*x*x);
	P.at<double>(1) = y*(1+k1*r+k2*r*r+k3*r*r*r) + 2*p2*x*y + p1*(r+2*y*y);

#ifdef VERBOSE
	ezPrintMat(intriMatG*P);
#endif
	P = intriMatG*P;
	return P;
}

